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Abstract: This paper presents a novel Long Baseline (LBL) position and velocity navigation 
filter for underwater vehicles based directly on the sensor measurements. The solution departs 
from previous approaches as the range measurements are explicitly embedded in the filter design, 
therefore avoiding inversion algorithms. Moreover, the nonlinear system dynamics are considered 
to their full extent and no linearizations are carried out whatsoever. The filter error dynamics 
are globally asymptotically stable (GAS) and it is shown, under simulation environment, that 
the filter achieves similar performance to the Extended Kalman Filter (EKF) and outperforms 
linear position and velocity filters based on algebraic estimates of the position obtained from 
the range measurements. 
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1. INTRODUCTION 

Accurate navigation systems are essential for the suc- 
cessful operation of autonomous vehicles. Although there 
exist alternatives such as terrain-based navigation, most 
navigation systems contain an Inertial Navigation System 
(INS) that provides the state of the vehicle by integrating, 
in open-loop, the information provided by inertial sensors, 
e.g., accelerometers and rate gyros. Although INS provides 
very good short term results, its performance necessarily 
degrades over time, not only due to the integration of 
sensor noise but also due to sensor bias errors. In order to 
overcome these drawbacks, aiding devices are considered 
to correct INS errors. This paper addresses the problem 
of vehicle navigation using ranges to a set of landmarks 
disposed in a Long Baseline (LBL) configuration. 

Among the myriad of aiding devices, the Global Posi- 
tioning System (GPS) is a very popular choice, see, e.g., 
Sukkarieh et al. (1999) Yun et al. (1999), and Vik, B. 
and Fossen, T. (2001). For underwater vehicles, GPS is 
not a solution due to the strong attenuation that the 
electromagnetic field suffers in water. Therefore, other 
solutions have been sought in the past, including acoustic 
positioning systems like LBL and Short Baseline (SBL), 
see Jouffroy and Opderbecke (2004), Kinsey and Whit- 
comb (2003), Larsen (2001), Larsen (2000), Vaganay et al. 
(1998), and references therein. In Youngberg (1992) the 
author proposes a GPS-like system consisting of buoys 
equipped with DGPS receptors. A related solution, de- 
nominated as GPS Intelligent Buoy (GIB) system, is now 
commercially available, see Thomas (1998). Further work 
on the GIB underwater positioning system can be found in 
Alcocer et al. (2007). Position and linear velocity globally 
asymptotically stable (GAS) filters based on a Ultra-Short 
Baseline (USBL) positioning system were presented by the 



authors in Batista et al. (2008). For interesting discussions 
and more detailed surveys on underwater vehicle navi- 
gation and sensing devices see Kinsey et al. (2006) and 
Leonard et al. (1998). 

This paper presents a position and linear velocity nav- 
igation filter based on range measurements disposed in 
a LBL configuration. Traditional solutions resort either 
to the Extended Kalman Filter (EKF) or to solutions 
based on position algebraic estimates obtained from the 
range measurements. The solution presented in the paper 
departs from previous approaches as the range measure- 
ments are explicitly embedded in the filter design, there- 
fore avoiding inversion algorithms. Moreover, the nonlinear 
system dynamics are considered to their full extent and no 
linearizations are carried out whatsoever, which allows to 
show that the filter error dynamics are globally asymptot- 
ically stable. Central to the proposed filtering framework 
is the derivation of a linear time-varying (LTV) system 
that captures the dynamics of the nonlinear system. The 
LTV model is achieved through appropriate state augmen- 
tation, which is shown to mimic the nonlinear system. 
Applications of the proposed solution are many and, under 
simulation environment, it is shown that the filter achieves 
similar performance to the Extended Kalman Filter (EKF) 
and outperforms linear position and velocity filters based 
on position algebraic estimates obtained directly from the 
range measurements. 

The paper is organized as follows. The problem statement 
and nominal system dynamics are introduced in Section 2, 
while the filter design is detailed in Section 3. Simulation 
results are presented in Section 4 and Section 5 summarizes 
the main conclusions of the paper. 



1.1 Notation 

Throughout the paper the symbol 0„xm denotes an n x m 
matrix of zeros, I„ an identity matrix with dimension n x 
n, and diag(Ai, . . . , An) a block diagonal matrix. When 
the dimensions are omitted the matrices are assumed of 
appropriate dimensions. If x and y are two vectors of 
identical dimensions, x x y and x • y represent the cross 
and inner products, respectively. 

2. PROBLEM STATEMENT 

Consider an underwater vehicle moving in a scenario 
where there is a set of fixed landmarks installed in a 
Long Baseline configuration and suppose that the vehicle 
measures the range to each of the landmarks, as depicted 
in Fig. 1. Further assume that the vehicle is equipped with 




Fig. 1. Mission Scenario 

an Inertial Measurement Unit (IMU), consisting of two 
triads of orthogonally mounted accelerometers and rate 
gyros, and an Attitude and Heading Reference System 
(AHRS). The problem considered in the paper is the design 
of a sensor-based globally asymptotically stable filter to 
estimate the position and linear velocity of the vehicle. 

2.1 System Dynamics 

In order to detail the problem framework, let {/} denote an 
inertial reference coordinate frame and {B} a coordinate 
frame attached to the vehicle, commonly denominated as 
the body-fixed coordinate frame. The linear motion of the 
vehicle is described by 

p(t) - R(i)v(t), (1) 

where p G W" denotes the inertial position of the vehicle, 
V e M'^ is the velocity of the vehicle relative to {/} 
and expressed in body-fixed coordinates, and R is the 
rotation matrix from {B} to {/}, which satisfies R(t) = 
R(i)S (a;(i)), where a; e R"^ is the angular velocity of {B}, 
expressed in body- fixed coordinates, and S (w) is the skew- 
symmetric matrix such that S (a;) a; is the cross product 

bJ X X. 

The AHRS provides the attitude rotation matrix R and 
the angular velocity w, while the IMU, assumed mounted 
at the center of mass of the vehicle, measures the linear 
acceleration a, which satisfies 

a(i)=v(0 + S(u;(i))v(t)-g(t), (2) 

where g £ R'^ denotes the acceleration of gravity, expressed 
in body-fixed coordinates. Ideal accelerometers would not 
measure the gravitational term but in practice this term 



must be considered due to the inherent physics of the ac- 
celerometers, see Kelly (1994) for further details. Since the 
magnitude of g is usually well known, it would be possible 
to cancel this term in (2) as the attitude of the vehicle is 
measured. However, even small errors on R would lead to 
large errors in the acceleration compensation. Therefore, 
the acceleration of gravity is considered here as unknown 
state, in addition to p and v. The term S {ijj(t)) v(i) 
corresponds to the Coriolis acceleration and cannot be 
neglected, particularly for vehicles that execute aggressive 
maneuvers. Finally, let s^ G M^, i — 1, ..., n^,, denote 
the inertial positions of the landmarks. Then, the range 
measurements are given by 

r,(i) = ||s,-p(i)||. (3) 

The derivative of the velocity can be written, from (2), as 

v(t)=a(i)-SMi))v(i)+g(i). (4) 

The derivative of g, assuming that the acceleration of 
gravity is constant in inertial coordinates, is given by 

g(i) = -S(u;(i))g(t). (5) 

Combining (1) and (3)-(5) yields the nonlinear system 

f p(t) = R(i)v(i) 
v(i)=a(t)-S(a;(t))v(t)+g(t) 
g(t) = -S(a;(t))g(i) 
ri(t) = ||si-p(t)|| . (6) 

TnAt) = INni -PWII 

The problem considered in the paper is the design of a 
filter for (6) assuming noisy measurements. 

In the accelerometer measurements (2) it was not con- 
sidered accelerometer bias. Its inclusion in the system 
dynamics is trivial but the observability analysis would 
result even more tedious. Nevertheless, the observability 
of linear motion quantities considering accelerometer bias 
was previously studied by the authors in Batista et al. 
(2009a) and it is rather straightforward to conclude about 
the necessary (and sufficient) conditions for observability 
of the system considering also accelerometer bias. For the 
sake of simplicity and clarity of presentation, which focuses 
on the novel Long Baseline solution, the accelerometer is 
assumed to be calibrated in this paper and the overall 
system analysis and design will be considered elsewhere. 

2.2 Long Baseline Configuration 

Long Baseline acoustic configurations have been widely 
used in the past in the design of navigation systems. In 
the remainder of the paper the following assumption is 
considered: 

Assumption 1. There exists at least 4 noncoplanar land- 
marks. 

When there exist at least 4 noncoplanar landmarks, it is 
always possible to determine the position of the vehicle 
from the range measurements. When there are fewer mea- 
surements that may not always be possible. For example, 
for a static vehicle and 3 landmarks, there exist two pos- 
sible solutions. Although it is assumed that there are at 
least 4 noncoplanar landmarks, the proposed solution is 
general and a more detailed discussion on different LBL 
configurations will be presented in the sequel. 



3. FILTER DESIGN 

This section presents the main results of the paper. Firstly, 
a state transformation is applied, in Section 3.1, to reduce 
the complexity of the system dynamics. Afterwards, state 
augmentation is proposed, in Section 3.2, in order to 
derive sensor-based deterministic system dynamics, and 
in Section 3.3 the observability of the resulting system 
is analyzed. The design of a Kalman filter is detailed 
in Section 3.4 and finally, in Section 3.5, some practical 
considerations are presented. 



3.1 State Transformation 

Let T(i) := diag (I, R(t),R(t)) and consider the state 
transformation 



Xi(t) 



T(t) 



P(i) 
v(i) 

g(i) 



(7) 



which is a Lyapunov state transformation previously used 
by the authors, see Batista et al. (2009b). The new system 
dynamics are given by 



f Xi(t)=X2(i) 

i2(t)=X3(i)+u(t) 

ri(i)-||si-xi(t)|| 

Tndt) = llSni -Xi(i)|| 



(8) 



where u(i) := R(i)a(i). Notice that, as (7) is a Lyapunov 
state transformation, all observability properties are pre- 
served Brockett (1970). The advantage of considering the 
state transformation (7) is that the new system is time 
invariant, although it is still nonlinear. 



3.2 State Augmentation 

To derive a linear system that mimics the dynamics of the 
nonlinear system (8), define n^, + 4 additional scalar state 
variables as 



( Xi{t) :==ri(i) 



x„^+4(i) := xi(i) •X2(t) 
XnL+5{t) :=xi(i) •X3(i) 

x„^+6 :=X2(i) •X3(i) 
x„,+7(t):-||x3(t)f 

and denote by 

a;(t)=[xf(i)x^(t)x|^(i)x4(t).. 



|x2(t)| 






n = 13 + rij^, the augmented state. It is easy to verify that 
the augmented dynamics can be written as 



x(t) == A{t)x{t) +Bu(i) 



where 



A(t) = 






I 





0. 


.0 





000 








I 


0. 


.0 





000 



n 






n 


0. 

n 


.0 

n 




1 


000 

. nnn 



n{t) 



u^(i) 











2u^(t) 



T 



0. 

0. 
0. 

u^(t)0. 
0. 



nit) 



--000 

100 




030 
001 
000. 



(9) 



and B = [0 1 ... 0] 

In order to complete the augmented system dynamics, 
notice first that the states X4, . . . , x^^nL ^-re measured. 
In addition to that, it is straightforward to show that 



2(si -Sj) ■p(^) 
n{t)+rj{t) 
or, equivalently, 

2(si -Sj) •xi(i) 



+ n{t)-r,{t) = 



n{t) + rj{t) 



+ .T3+j(t) -X3+j(t) == 



II l|2 II ||2 


n{t) + r,{t) 


1 l|2 II ||2 


n{t) + r,{t) ' 



(10) 
where the right side of (10) is known and the left side 
depends on the system state. Discarding the original non- 
linear system output, it is possible to write an augmented 
system output as 

fyi{t)=xi(t) 



2(si -S2)-Xl(t) 

n(t)+r2(t) 
2(si -S3)-xi(t) 



VuL + l 
ynL+2 



+ Xs+i(t) - 3:3+2 (t) 
+ 3:3 + 1 (t) - 3:3+3 (t) 



2(s 



■XI (t) 



:, + C, 



2(s„^_l 



-2{t)+rn^{t) 
-Snt)-Xl(t) 



+ 3:3 + ni-2(t)-2:3+n_c(t) 



nt+C, 



r„j,_i 



(t) + ^ni (t) 



+ 3:3 + ni-l(t)-a;3+n_c {*) 



where C^^ is the number of 2-combinations of n^ elements, 

i.e., C^^ = 2 ■ ^^ compact form, the augmented 

system dynamics are given by 

" x{t) = A(t)x(t) +Bu(t) 
^ y(t) = C{t)x{t) 

where 



C{t) = 



OriiXS 0„j^x3 0„^x3 


In, 


0,1^x4 


_ Ci(t) Oc^L^^ Oc^^^xs 


C2 


0cr-x4j 



(11) 

(12) 



Ci(i) 



2(si 



S2 



riit)+r2{t) 
2 (si ~ 83)^ 
riit)+r3{t) 

2 {SnL-2 — Sni) 



r„^_2(t)+r„^(i} 

rn^-l{t)+rrr^{t) 



pni, x3 



and 



C2 = 



1 -1 

1 



-1 







-1 

1 -1 



is the matrix that encodes ah the possible combinations of 
differences of pairs of ranges. 

The dynamic system (11) can be regarded as a hnear time- 
varying system, even though the system matrices A(i) and 
C(i) depend exphcitly on the system input and output, 
as evidenced by (9) and (12). However, these are known 
signals and therefore pose no problem, other than the fact 
that the observability of the system may depend on the 
system input, which does not happen for linear systems 
whose system matrices do not depend on the system input. 
Also, notice that there is nothing in (11) imposing 

f ri(i) = ||si-xi(i)|| 
rnjt) ^ \\Sn^ - :iii{t)\\ 

X„^+4(<)=Xl(t)-X2(i) • (13) 

X„^+5(0=Xl(i)-X3(i) + |[x2(t)f 
XriL+eit) =X2(t) ■X3(t) 
.X„,+7(t) = ||x3(t)f 

These restrictions could be easily implemented but this 
form is preferred since it allows to consider the system as 
linear. However, care must be taken when extrapolating 
conclusions from the observability of (11) to the observ- 
ability of (8) or (6). Finally, the following assumption is 
required so that (9) is well defined, which is not restrictive 
from the practical point of view since it would make 
no sense to have the vehicle at the same position of a 
landmark, where an acoustic transponder is installed. 

Assumption 2. The motion of the vehicle is such that 

3 V : r„ < Tiit). 

rm > t > to 



3.3 Observability Analysis 

In the previous section a LTV system was derived that 
aims to capture the behavior of the original nonlinear 
system. The analysis of the observability of this LTV 
system is carried out in this section and its behavior 
compared with that of the nonlinear system. 

In order to proceed with the analysis of the observability of 
the LTV system (11), it is convenient to compute the ob- 
servability Gramian associated with the pair (A(t),C(t)) 
and, in order to do so, the transition matrix associated 
with the system matrix A(t). Let 



u[^l(t,to):= / u{a)d(T 

Jto 



and 



u[2l(i,io):= / / u{a2) da2da 

J to J tf\ 



Long, but straightforward, computations show that the 
transition matrix associated with A(t) is given by 



where 



<l>{t,U) 



(f>AAit^to) 



(l)AAit,to) 

4>BA{'t,'to) I </>i5c(*>^o) 
.(f^CAi^lQ 0cc(t,to). 



I(t-to)I^^I 

I (t-t„)i 

I 



4>BA [t, to) = [ (}>BA1 (t, t«) 0BA2 (^: ^o) (/) B A3 (*> ^o) ] , 



4>BAl(t^to) 



* [uW(^,^o)]' 

to '^1 (^) 

* [uW(^,io)] 



■da 



da 



to 



ho r^L {^) 

(t>BA2{t,to) = 

-sf + (a-to)[uW(a,to)]'^+[u[2l(a,to)]- 
ri(cr) 



k 



t cT 



sl, + ia-to)[uM{a,to)Y +[ui'H<yMy 



ruL (c^) 



4'BA3(t''to) ■ 



■da 



-da 



_sf + (i^ [uW (a, to)] ^ + pl (a, t„)l ^ 

(ct - to) da 

ri (a) 



{(7 — to) : — ^ acr 



r^L (f^) 






ri i^) /, ri (a) J 2 n (a) J 2 n {a) 

•'to •''o •'to 



-da 



^-to_^I^H^-tof,fH<^~tof,^ 



to''"^^'^) •/to''"^('') •/to^ '""^('^) Jto"^ '""^('') 

and 4>cA (t,to) and (p^^ {t,to) are omitted for the sake of 
simplicity, as they are not required in the sequel. The ob- 
servability Gramian associated with the pair {A{t),C{t)) 
is simply given by 

W (to, tf) ^ f ' cj,^ (i, to) C^(t)C(t)0 (t, to) dt. 

Jto 



The following theorem establishes the observability of the 
LTV system (11). 

Theorem 1. The linear time- varying system (11) is observ- 
able on [to,ty], to <tf. 

Proof. The proof follows by contradiction. Suppose that 
the LTV system (11) is not observable on I :— [to,t/]. 
Then, there exists a nonnuU vector d G ]R^'^+"^ 

d^[d^dldldld^d^djd^]' , 



with di, ds, dg G R^, d4 G M"^, 4, . . . , ds G M, such 
that d-^W (toj ^) d = for all i G I or, equivalently, 



/ ||C(T)0(T,i„)df dr-O. 

J to 



(14) 



Taking the time derivative of (14) yields 
||C(i)</.(i,io)df = 0, 
which implies, in particular, that 

C(t)0(i,to)d = O 
for all i G I. Substituting t = to in (15) gives 

d4 

Ci(io)di + C2d4 

which implies immediately that 

d4 = 0. 

Substituting (17) in (16) gives 

(si -82)^ 







(15) 
(16) 
(17) 



n (to) + r"2 (io) 

2 , _ .T 

2 



di = 0. (18) 



/, \ I /, \ (S„^_2 ^Ul) 

rnL-2 [to) + rni^ (to) 

2 _ T 

T-Ki-l {to)+rni^ [to) 
It is straightforward to show that, under Assumption 1, 
the only solution of (18) is 

di=0. (19) 

Now, from (15), it is possible to write 



^[C(t)0(t,fo)d] = O 



(20) 



for all t G I. Expanding (20), and considering (17) and 
(19), allows to write 

-sf da + {t- to) [u[il (t, to)] ^ da + [u[21 (i, to)] "^ da 



+ (t - to) ■ 



n{t) 

"d3 + ii^ [uW (t, to)] ^ d3 + [uPl (t, to)] ^ d3 



n(t) 

1 *-to , , 3(t-to)^ , , 1 (t-to)'^ 



-do - 



-d7 + -- 



nit) nit) 2 n{t) 2 nit) 

for all i = 1, . . . , rii, t G / or, equivalently. 



ds = 



-si'd2 + {t-to) uW(t,i, 



d2 + 



ul^l(f,to) 



(t-t„)sfd3+^* '°^ 



,[2]r 



2 

T 



uW(i,to) 



+ (t - io) [ul^l (t, io)J da + dg + (t - to) do 

+ ^(t-to)'d7 + ^(i-to)'c?8 = 0. 

Substituting t = to in (21) gives 



(21) 



^1 


1] 






^2 


1 




rd2i 




1 
1_ 




[4 J 



(22) 



Again, it is straightforward to show that, under Assump- 
tion 1, the only solution of (22) is 



d2 

da -■ 









(23) 



Now, considering (23) in (21) and taking its time derivative 



gives 



-s, d. 



(t - toY 



[u{t)Y d3 + {t-to) uW(t,to) 



(t-to) uW(t,to) 



u[2l(f,to) 



rffi 



+3(t-to)d7 + ^(t-to)'d8=0 (24) 

for ah i = 1, . . . , Ul and t G /. With t = to in (24), 



(25) 



Again, under Assumption 1, the only solution of (25) is 

d3 = 

de = ■ 

Finally, substituting (26) in (24) gives 





1' 
1 




[da] 




1 
1_ 




[de\ 



(26) 



3 (t - to) dr+'^{t- t„f ds = (27) 

for all t E I. Since t — to and (t — to) are linearly 
independent functions, the only solution of (27) is dy = 
ds — 0. But that contradicts the hypothesis that there 
exists a nonnuU vector d such that (14) is true. Therefore, 
the LTV system (11) is observable. 

The fact that (11) is observable does not mean that the 
nonlinear system (8) is observable nor that an observer for 
(11) is also an observer for (8), as there is nothing in the 
system dynamics (11) imposing the algebraic restrictions 
(13). However, this turns out to be true, as shown in the 
following theorem. 

Theorem 2. The nonlinear system (8) is observable in 
the sense that, given the output {y(t), t G [to,^/]} and 
the input {u(t), t G [to, t/]}, the initial state x(to) = 

[xf (to) x^ (to) x|" (to)] is uniquely defined. Moreover, 
a state observer for the LTV system (11) with globally 
asymptotically stable error dynamics is also a state ob- 
server for the nonlinear system (8), with globally asymp- 
totically stable error dynamics. 

Proof. It has been shown, in Theorem 1, that the LTV 
system (11) is observable. Therefore, given the output 
{y(t), t G [to,t/]} and the input {u(t), t G [to,t/]}, the 
initial state of (11) is uniquely defined. Let 

Z (to) = [zj (to) Z^ (to) zj (to) Z4 (to) . . . Zn^+7 (to)] 

zi, Z2, Z3 G M'', Z4, . . . , 2„^+7 (to) G R, be the initial state 

of (11) and X (to) = [x^ (to) x^ {to) xj^ (to)] be the initial 
state of the nonlinear system (8). It is easy to show that 
the evolution, for the nonlinear system, of xi(t), is given 

by 



Xi(t)^xi(to) + (t-io)x2(to) + 



{t - toY 



X3 (to) 



(28) 



while the output of the nonhnear system satisfies 

rf(t) = ||xi (io) - s, f + (t - io)' ||X2 (io) f 

+ ^^^^ I|X3 (to) f + 2 (t - to) XI (to) • X2 (to) 

+ (t - to)' Xi (to) • X3 (to) + (t - to)-'' X2 (to) • X3 (to) 
-2 (t - to) Si • X2 (to) - (t - to)' Si ■ X3 (to) 

+2x1 (to) • uM {t, to) + 2x2 (to) • (t - to) uPl {t, to) 

+X3(to)-(t-to)'u[2l(t,to)+||u['l(t,to)|| 

-2s,-u[2l(t,io). (29) 

Therefore, from (28) and (29), it is triviahy shown that 



rUt) - r^{t) + 2 (s, - s,) ■ xi(t) = ||xi (to) - s, 



(30) 



- ||xi (to) - Sj II + 2 (si - Sj) ■ xi (to) 

for all i, j e {1, . . . , rif,} and t e I. Now, notice that, for 
the LTV system, multiplying the set of augmented outputs 
2/„j^+i, . . . , 2/^ +c"^ ^y ^^^ corresponding sums of pair of 
ranges yields 

r--(i)-'^'(i) + 2(s,-s,).xi(t), 
while its evolution can be shown to satisfy 

rKt) - r]{t) + 2 (s, - s,) ■ xi(t) = zl+, (to) - zl+^ (to) 

+2 (s, - s,) • zi (to) (31) 

for all i, j £ {1, . . . , Ul} and t E I. The states of the 
augmented LTV system X4(t), . . . , XnL+rit) are actually 
measured and correspond to the range measurements. 
Therefore, it must be 

23+i(to) = ||xi (to) -Sill (32) 

for alH S {1, . . . , Ul}. Then, from the comparison of (30) 
and (31), and considering (32), it follows that 



(Sl -S2) 
(Sl - S3)^ 

y^riL — l ^n^ 



[xi (to) - zi (to)] = 0. 



(33) 



Under Assumption 1 the only solution of (33) is 

xi (to) == zi (to) . 
From (29) it is possible to write 

^'(0 -r|(t) = ||xi(to) -s,f - ||xi(to) -s,f 

-2(t-to)(Si -Sj) ■X2(to) 
-(t-to)'(Si -Sj) ■X3(to) 

-2(s,-s,)-uPl(t,io) (34) 

for all I, j G {1, . . . , n^} and t G I. On the other hand, 
the evolution of the square of range for the LTV system 
(11) can be written as 



r2(t) = 2 [zi (to) -s,]-u[2l(i, to) 

-2 (t - to) S, ■ Z2 (to) + 2 (t - to) u[2l (i, to) . Z2 (to) 

-it- to)' S, • Z3 (to) + (t - to)' UPl (t, to) ■ Z3 (to) 

+4+^ito)+2{t-to)Zr,,+i{to) 

+ (t — to) Znj^+5 (to) + (t — to) Znj^+e (to) 



(t - to)' 



for alH e {1, 



. , "t 



2ni,+7(to)+ Upl(t,io) 



}. Therefore, it is possible to write 



rf{t)-r^{t)=zl+,{to)-zl+^{to) 

-2 (t - to) (Si - Sj) ■ Z2 (to) 
- (t - to)' (Sj - Sj) ■ Z3 (to) 

-2(s, -s,)-u[2l(i,to) (35) 

for all i,j £ {1, ..., Ul} and t £ I. Taking the time 
derivative of both (34) and (35) and comparing for t = to 



gives 



(si -S2) 
(si - S3) 

(Sni-2 — S, 
V^nj: — 1 Sy 



[X2 (to) - Z2 (to)] = 0. 



(36) 



Again, under Assumption 1 the only solution of (36) is 

X2 (to) = Z2 (to) . 

Finally, taking the second time derivative of both (34) and 
(35) and comparing for t — to gives 



(Sl -S2j 
(Sl - S3) 



(Stij^-2 — S^ 



[X3 (to) - Z3 (to)] = 0. 



(37) 



Again, under Assumption 1 the only solution of (37) is 
X3 (to) — Z3 (to). This concludes the proof, as the initial 
state of the LTV system (11), which is uniquely defined, 
matches the initial state of the nonlinear system (8). 

Remark 1 . The concept of observability for nonlinear sys- 
tems is not as strong as that presented in the statement of 
Theorem 2, see Hermann and Krener (1977). That is the 
reasoning behind explicitly describing in what sense the 
system is observable. 

Remark 2. In the proof of Theorem 2 it was not shown 
that all the algebraic relations (13) are satisfied. It was 
only shown that the initial state of the LTV system (11) 
coincides with the initial state of the nonlinear system 
(8). However, it is trivial to show that those relations are 
indeed preserved. 

Remark 3. Before concluding this section, it is important 
to remark that, although the observability results were 
derived with respect to the nonlinear system (8), they 
also apply to the original nonlinear system (6) as they 
are related through a Lyapunov transformation. Also, the 
design of an observer for the original nonlinear system 
follows simply by reversing the state transformation (7), 
as it will be detailed in the following section. 



3-4 Kalman Filter 

Although ah the results derived so far were presented in a 
deterministic setting, in practice there exists measurement 
noise and often system disturbances. Therefore, a filtering 
solution is proposed in this section instead of an observer. 
On the other hand. Theorem 2 provides a constructive 
result in the sense that a dynamic system with globally 
asymptotically stable error dynamics for the LTV system 
(8) provides globally asymptotically stable error dynamics 
for the estimation of the state of the nonlinear system. 
Therefore, the design of a Kalman filter follows for the 
LTV system (11), albeit other solutions could be devised, 
e.g., an "Hoc filter. It is important to stress, however, that 
this filter is not optimal. Indeed, looking into the system 
matrices, it is easy to see that, in the presence of noise in 
the range or acceleration measurements, there exists mul- 
tiplicative noise. Nevertheless, the Kalman filter is GAS, 
as it is straightforward to show that the system under 
consideration is not only observable but also uniformly 
completely observable. 

In order to recover the augmented system dynamics in 
the original coordinate space, consider the augmented 
state transformation x(t) = Tc(t)a;(t), where Tc(i) = 
diag (l,R"^(t), R"''(t), 1, . . . , l). Then, the nominal aug- 
mented system dynamics in the original coordinate space 
are given by 



lxit)=A{t)x{t)+^{t)a.{t) 
\y{t) = C{t)x{t) 



where 
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Including system disturbances and sensor noise to tune the 
Kalman filter gives the final system dynamics 

X{t) = A{t)x{t) + B{t)a{t) + n,(i) 
y{t) = C{t)x{t) + ny{t) 

where it is assumed that n^ and riy are uncorrelated zero- 
mean Gaussian noise, with E [nx{t)n'^ (r)] = Q^S {t — t) 
and E [uy (t) n^ (r)] = QyS {t - t) . 

3.5 Practical Considerations 



It was assumed in the paper that there were at least 
4 noncoplanar landmarks, in order to fit the configura- 
tion of a Long Baseline acoustic setup. Nevertheless, the 
proposed solution is general and can be applied to any 
number of landmarks, including the case of single range 
measurements. This last setup was already studied by the 



authors, see Batista et al. (2009c) and Batista et al. (2010). 
The remaining cases of 2 and 3 landmarks differ only in 
the observability analysis. When there is a single range 
measurement, it was shown in Batista et al. (2010) that 
the corresponding LTV system would be observable if and 
only if the set of functions 



to, (t-taf , {t-taf , {t-tof, 



Pi (<) - Pi (to ) , P2 (t) - P2 (to ) , P3 (t) - P3 (*0 ) , 

(i ~ to) [Pl(t) - PI (to)] , (t - to) [p2(t) - P2 (to)] , 

(t - to) [P3(t) -P3 (to)] , (t - tof [pi{t)~pi (to)] , 

(t - tof \p2{t) - P2 (to)] , (t - to)^ [P3(t) - P3 (to)]} 

is linearly independent on [to,t/]. The cases of 2 and 
3 landmarks require less demanding conditions so that 
the corresponding LTV systems are observable. These 
conditions will be presented elsewhere. Nevertheless, the 
design follows the same steps. 

4. SIIVIULATION RESULTS 

In order to evaluate the performance achieved with the 
proposed navigation solution, simulations were carried out 
using a kinematic model for an underwater vehicle. The 
fact that the full nonlinear dynamics of the vehicle are 
not considered is not a drawback as the proposed filter 
relies solely on the vehicle kinematics, which are exact. 
Therefore, the proposed solution applies to any underwater 
vehicle, independently of the particular dynamics. 

The trajectory described by the vehicle is shown in Fig. 2. 




y(m) 



x{m) 



Fig. 2. Trajectory described by the vehicle 



Sensor noise was considered for all sensors. In particular, 
the range, acceleration, and angular velocity measure- 
ments are assumed to be corrupted by additive uncor- 
related zero-mean white Gaussian noise, with standard 
deviations of 1 m, 2 x 10~^ m/s^, and 0.05 °/s, respectively. 
The attitude, parameterized by roll, pitch, and yaw Euler 
angles, was also assumed to be corrupted by zero-mean 
additive white Gaussian noise, with standard deviation of 
0.03° for the roll and pitch and 0.3° for the yaw. 



The LBL configuration is composed of 4 acoustic transpon- 
ders and their positions are 



Si = 







1000 



M, S2 = 



" 
S3 = 1000 (m), S4 
.1000. 
which satisfy Assumption 1. 



1000 


1000 

" - 


500 



(m), 



(m), 



To tunc the Kalman filter, the state disturbance intensity 
matrix was chosen as Qa; = 10~^I and the output noise 
intensity matrix as Qy = diag (1, 1, 1, 1, 2, 2, 2, 2, 2, 2). The 
initial conditions were set to zero for the position and 
velocity. The acceleration of gravity was initialized closed 
to the true value, with [0 10] m/s^ as the attitude is 
measured and the magnitude of the acceleration of gravity 
is usually known. Notice that it would be possible to 
initialize the position with a close estimate obtained from 
the inversion of the first set of LBL range measurements. 
The states corresponding to the range measurements were 
initialized with the first set of measurements while the 
remaining states were set to zero, apart from xn, which 
corresponds to the square of the magnitude of the accel- 
eration of gravity, which was initialized with 100. 

The initial evolution of the position, velocity, and acceler- 
ation of gravity errors is depicted in Fig. 3, whereas the 
initial evolution of the range errors is shown in Fig. 4. The 
initial convergence of the remaining state errors is shown 
in Fig. 5. As it can be seen from the various plots, the 
convergence rate of the filter is quite fast. 
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Fig. 3. Initial convergence of the position, velocity, and 
acceleration of gravity error 




Fig. 4. Initial convergence of the range errors 
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Fig. 5. Initial convergence of the error of the augmented 

states xg, xg, xio, and xn 
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Fig. 6. Detailed evolution of the position, velocity, and 
acceleration of gravity error 

The proposed filter was compared against two different 
solutions: the first is the well-known Extended Kalman 
Filter; the second consists in applying the linear Kalman 
filter proposed in Batista et al. (2009b) using a position 
algebraic estimate obtained from the range measurements 
to feed the filter. Unfortunately, it is not possible to show 
the results here due to the lack of space, but in short, the 
performance of the proposed solution is similar to the EKF 
and outperforms the one based on the algebraic inversion 
of the problem. These results will be shown in an extended 
version of the paper. 



In order to better illustrate the performance achieved 
with the proposed solution, the steady-state errors of the 
position, velocity, and acceleration of gravity are shown in 
Fig. 6. Notice that the errors are confined to very tight 
intervals, in spite of the realistic measurement noise. 



5. CONCLUSIONS 

This paper presented a novel Long Baseline (LBL) position 
and velocity navigation filter for underwater vehicles based 
directly on the sensor measurements. Traditional solutions 



resort either to the Extended Kalman Filter (EKF) or to 
solutions based on position algebraie estimates obtained 
from the range measurements. The solution presented 
in the paper departs from previous approaehes as the 
range measurements are explicitly embedded in the filter 
design, therefore avoiding inversion algorithms. Moreover, 
the nonlinear system dynamics are considered to their full 
extent and no linearizations are carried out whatsoever, 
which allows to show that the filter error dynamics are 
globally asymptotically stable. State augmentation is at 
the core of the proposed framework. Indeed, the proposed 
filter has 13 + n^ states, which compares to a minimum 
of 9 states for traditional solutions. It is the opinion of 
the authors that the achieved performance coupled with 
the guarantee of global asymptotic stability justify the 
additional computational complexity, at least for those 
missions where extreme constraints on power do not exist. 

Under simulation environment it was shown that the filter 
achieves similar performance to the Extended Kalman 
Filter (EKF) and outperforms linear position and velocity 
filters based on position algebraic estimates obtained di- 
rectly from the range measurements. The advantage over 
the EKF is however clear due to the GAS property of the 
proposed solution, which is not guaranteed for the EKF. 

Finally, although sensor bias, in particular, accelerometer 
bias, is a very important problem, in this paper it is as- 
sumed, for the sake of clarity of presentation and simplicity 
of design, that the accelerometer has been previously cali- 
brated. Nevertheless, it is easy to include the accelerometer 
bias in the system design, which leads to observability con- 
ditions so that it is possible to estimate both the gravity 
and the bias, according to previous work by the authors. 
The overall system design and observability analysis will 
be presented elsewhere. 
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